#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <geometry_msgs/msg/pose.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

void scale_trajectory_speed(moveit::planning_interface::MoveGroupInterface::Plan &plan, double scale)
{
    int n_joints = plan.trajectory_.joint_trajectory.joint_names.size();

    for (size_t i = 0; i < plan.trajectory_.joint_trajectory.points.size(); i++)
    {
        double time_in_seconds = plan.trajectory_.joint_trajectory.points[i].time_from_start.sec +
                                 plan.trajectory_.joint_trajectory.points[i].time_from_start.nanosec / 1e9;
        time_in_seconds /= scale;

        plan.trajectory_.joint_trajectory.points[i].time_from_start.sec = static_cast<int32_t>(time_in_seconds);
        plan.trajectory_.joint_trajectory.points[i].time_from_start.nanosec =
            static_cast<uint32_t>((time_in_seconds - plan.trajectory_.joint_trajectory.points[i].time_from_start.sec) * 1e9);

        for (int j = 0; j < n_joints; j++)
        {
            plan.trajectory_.joint_trajectory.points[i].velocities[j] *= scale;
            plan.trajectory_.joint_trajectory.points[i].accelerations[j] *= scale * scale;
        }
    }
}

bool returnToMiddle(moveit::planning_interface::MoveGroupInterface &move_group, rclcpp::Logger logger) {
    move_group.setNamedTarget("middle");
    moveit::planning_interface::MoveGroupInterface::Plan home_plan;
    bool success = (move_group.plan(home_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    if (success) {
        move_group.execute(home_plan);
        RCLCPP_INFO(logger, "回到middle位置.");
        return true;
    } else {
        RCLCPP_ERROR(logger, "Failed to return to middle position.");
        return false;
    }
}

bool executeCartesianPath1(moveit::planning_interface::MoveGroupInterface &move_group, rclcpp::Logger logger) {
    if (!returnToMiddle(move_group, logger)) {
        RCLCPP_ERROR(logger, "Failed to return to middle position before executing Cartesian path.");
        return false;
    }
    RCLCPP_INFO(logger, "Starting Cartesian path execution");

    /* #region  */
    std::vector<geometry_msgs::msg::Pose> waypoints;
    // 目标点0 28/-45/90/-45/0
    geometry_msgs::msg::Pose pose0;
    pose0.position.x = 0.083024;
    pose0.position.y = 0.10987;
    pose0.position.z = 0.28763;
    pose0.orientation.x = 0;
    pose0.orientation.y = 0;
    pose0.orientation.z = 0.24475;
    pose0.orientation.w = 0.96959;
    waypoints.push_back(pose0);

    // 目标点1 28/-50/90/15/0
    geometry_msgs::msg::Pose pose1;
    pose1.position.x = -0.034461;
    pose1.position.y = 0.046522;
    pose1.position.z = 0.10748;
    pose1.orientation.x = -0.11163;
    pose1.orientation.y = 0.44231;
    pose1.orientation.z = 0.21777;
    pose1.orientation.w = 0.86283;
    waypoints.push_back(pose1);

    // 目标点2 28/-60/100/10/0
    geometry_msgs::msg::Pose pose2;
    pose2.position.x = -0.033193;
    pose2.position.y = 0.047216;
    pose2.position.z = 0.096255;
    pose2.orientation.x = -0.10337;
    pose2.orientation.y = 0.40969;
    pose2.orientation.z = 0.22182;
    pose2.orientation.w = 0.87879;
    waypoints.push_back(pose2);

    // 目标点3 28/-65/110/0/0
    geometry_msgs::msg::Pose pose3;
    pose3.position.x = -0.016352;
    pose3.position.y = 0.056293;
    pose3.position.z = 0.087686;
    pose3.orientation.x = -0.093294;
    pose3.orientation.y = 0.36973;
    pose3.orientation.z = 0.22624;
    pose3.orientation.w = 0.89633;
    waypoints.push_back(pose3);

    // 目标点4 28/-70/119/-9/0
    geometry_msgs::msg::Pose pose4;
    pose4.position.x = -0.00072952;
    pose4.position.y = 0.064723;
    pose4.position.z = 0.083604;
    pose4.orientation.x = -0.082545;
    pose4.orientation.y = 0.32711;
    pose4.orientation.z = 0.23041;
    pose4.orientation.w = 0.91274;
    waypoints.push_back(pose4);

    // 目标点5 28/-78/134/-28/0 
    geometry_msgs::msg::Pose pose5;
    pose5.position.x = 0.030862;
    pose5.position.y = 0.081752;
    pose5.position.z = 0.082453;
    pose5.orientation.x = -0.059886;
    pose5.orientation.y = 0.23743;
    pose5.orientation.z = 0.23731;
    pose5.orientation.w = 0.94007;
    waypoints.push_back(pose5);

    // 目标点55 28/-78/136/-37/0 
    geometry_msgs::msg::Pose pose55;
    pose55.position.x = 0.045914;
    pose55.position.y = 0.08981;
    pose55.position.z = 0.10271;
    pose55.orientation.x = -0.045323;
    pose55.orientation.y = 0.17931;
    pose55.orientation.z = 0.24037;
    pose55.orientation.w = 0.9529;
    waypoints.push_back(pose55);

    // 目标点6 28/-78/138/-44/0
    geometry_msgs::msg::Pose pose6;
    pose6.position.x = 0.054854;
    pose6.position.y = 0.094613;
    pose6.position.z = 0.11793;
    pose6.orientation.x = -0.034817;
    pose6.orientation.y = 0.13775;
    pose6.orientation.z = 0.24206;
    pose6.orientation.w = 0.9598;
    waypoints.push_back(pose6);

    // 目标点7 28/-82/142/-52/0 半举平
    geometry_msgs::msg::Pose pose7;
    pose7.position.x = 0.061581;
    pose7.position.y = 0.098223;
    pose7.position.z = 0.14013;
    pose7.orientation.x = -0.017288;
    pose7.orientation.y = 0.068398;
    pose7.orientation.z = 0.24388;
    pose7.orientation.w = 0.96723;
    waypoints.push_back(pose7);

    // 目标点8 28/-82/145/-60/0 举平
    // 目标点8 28/-82/145/-58/0 举平
    geometry_msgs::msg::Pose pose8;
    pose8.position.x = 0.067408;
    pose8.position.y = 0.10135;
    pose8.position.z = 0.14599;
    pose8.orientation.x = -0.010242;
    pose8.orientation.y = 0.040522;
    pose8.orientation.z = 0.24425;
    pose8.orientation.w = 0.96881;
    waypoints.push_back(pose8);

    // 目标点8 28/-90/142/-50/0 刮板
    geometry_msgs::msg::Pose pose9;
    pose9.position.x = 0.053625;
    pose9.position.y = 0.093921;
    pose9.position.z = 0.16204;
    pose9.orientation.x = -0.0035733;
    pose9.orientation.y = 0.014151;
    pose9.orientation.z = 0.24446;
    pose9.orientation.w = 0.96955;
    waypoints.push_back(pose9);

    //第10个目标位姿 14/-90/142/-50/0 左移
    geometry_msgs::msg::Pose pose10;
    pose10.position.x = 0.071307;
    pose10.position.y = 0.047553;
    pose10.position.z = 0.16428;
    pose10.orientation.x = -0.0011452;
    pose10.orientation.y = 0.0093179;
    pose10.orientation.z = 0.12094;
    pose10.orientation.w = 0.99262;
    waypoints.push_back(pose10);

    //第11个目标位姿 0/-60/120/-60/0 low-middle
    geometry_msgs::msg::Pose pose11;
    pose11.position.x = 0.11027;
    pose11.position.y = 0;
    pose11.position.z = 0.226;
    pose11.orientation.x = 0;
    pose11.orientation.y = 0;
    pose11.orientation.z = 0;
    pose11.orientation.w = 1;
    waypoints.push_back(pose11);

/* #endregion */

    move_group.setStartStateToCurrentState();
    moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
    double fraction = move_group.computeCartesianPath(waypoints, 0.01, 0.0, cartesian_plan.trajectory_);
    
    RCLCPP_INFO(logger, "Trajectory %.2f%% achieved", fraction * 100.0);
    
    if (fraction >= 0.8) {
        scale_trajectory_speed(cartesian_plan, 1.4);
        move_group.execute(cartesian_plan);
        RCLCPP_INFO(logger, "Trajectory execution completed");
        return true;
    } else {
        RCLCPP_ERROR(logger, "Failed to plan Cartesian path sufficiently");
        return false;
    }
}

bool executeCartesianPath2(moveit::planning_interface::MoveGroupInterface &move_group, rclcpp::Logger logger) {
    if (!returnToMiddle(move_group, logger)) {
        RCLCPP_ERROR(logger, "Failed to return to middle position before executing Cartesian path.");
        return false;
    }
    RCLCPP_INFO(logger, "Starting Cartesian path execution");

    /* #region  */
    std::vector<geometry_msgs::msg::Pose> waypoints;
    // 目标点1 0/-50/90/15/0
    geometry_msgs::msg::Pose pose1;
    pose1.position.x = -0.011801240508450508;
    pose1.position.y = -1.6717636845861036e-05;
    pose1.position.z = 0.14428133950504773;
    pose1.orientation.x = -2.3927732160852933e-06;
    pose1.orientation.y = 0.3826546744537482;
    pose1.orientation.z = -5.627848297886214e-05;
    pose1.orientation.w = 0.9238914421866365;
    waypoints.push_back(pose1);

    // 目标点2 0/-60/100/10/0
    geometry_msgs::msg::Pose pose2;
    pose2.position.x = -0.022079;
    pose2.position.y = -7.74576e-05;
    pose2.position.z = 0.0950052;
    pose2.orientation.x = -6.4903e-05;
    pose2.orientation.y = 0.422351;
    pose2.orientation.z = -0.000237108;
    pose2.orientation.w = 0.906432;
    waypoints.push_back(pose2);

    // 目标点3 0/-65/110/0/0
    geometry_msgs::msg::Pose pose3;
    pose3.position.x = -0.0021559;
    pose3.position.y = -8.5636e-05;
    pose3.position.z = 0.087629;
    pose3.orientation.x = -1.2533e-05;
    pose3.orientation.y = 0.3815;
    pose3.orientation.z = -0.00027885;
    pose3.orientation.w = 0.92437;
    waypoints.push_back(pose3);

    // 目标点4 0/-70/119/-9/0
    geometry_msgs::msg::Pose pose4;
    pose4.position.x = 0.0151664;
    pose4.position.y = -9.93429e-05;
    pose4.position.z = 0.0808511;
    pose4.orientation.x = -4.666e-06;
    pose4.orientation.y = 0.344447;
    pose4.orientation.z = -0.000031351;
    pose4.orientation.w = 0.9388;
    waypoints.push_back(pose4);

    // 目标点5 0/-78/134/-28/0 
    geometry_msgs::msg::Pose pose5;
    pose5.position.x = 0.051718;
    pose5.position.y = -5.9906e-05;
    pose5.position.z = 0.084033;
    pose5.orientation.x = -0.00040868;
    pose5.orientation.y = 0.24246;
    pose5.orientation.z = -0.00023144;
    pose5.orientation.w = 0.97016;
    waypoints.push_back(pose5);

    // 目标点6 0/-78/138/-44/0
    geometry_msgs::msg::Pose pose6;
    pose6.position.x = 0.086392;
    pose6.position.y = 0;
    pose6.position.z = 0.14227;
    pose6.orientation.x = -0.00011136;
    pose6.orientation.y = 0.065582;
    pose6.orientation.z = -0.00032853;
    pose6.orientation.w = 0.99785;
    waypoints.push_back(pose6);

    // 目标点7 0/-82/142/-52/0 半举平
    geometry_msgs::msg::Pose pose7;
    pose7.position.x = 0.093833;
    pose7.position.y = 0;
    pose7.position.z = 0.15401;
    pose7.orientation.x = -5.9707e-05;
    pose7.orientation.y = 0.039548;
    pose7.orientation.z = -0.00001562;
    pose7.orientation.w = 0.99922;
    waypoints.push_back(pose7);

    // 目标点8 0/-82/145/-60/0 举平
    geometry_msgs::msg::Pose pose8;
    pose8.position.x = 0.093833;
    pose8.position.y = 0;
    pose8.position.z = 0.15401;
    pose8.orientation.x = -5.9707e-05;
    pose8.orientation.y = 0.039548;
    pose8.orientation.z = -0.00001562;
    pose8.orientation.w = 0.99922;
    waypoints.push_back(pose8);

    // 目标点8 0/-90/142/-50/0 刮板
    geometry_msgs::msg::Pose pose9;
    pose9.position.x = 0.077103;
    pose9.position.y = 0;
    pose9.position.z = 0.16429;
    pose9.orientation.x = -4.1935e-05;
    pose9.orientation.y = 0.0093845;
    pose9.orientation.z = -0.0011772;
    pose9.orientation.w = 0.99996;
    waypoints.push_back(pose9);

    //第10个目标位姿 14/-90/142/-50/0 左移
    geometry_msgs::msg::Pose pose10;
    pose10.position.x = 0.071307;
    pose10.position.y = 0.047553;
    pose10.position.z = 0.16428;
    pose10.orientation.x = -0.0011452;
    pose10.orientation.y = 0.0093179;
    pose10.orientation.z = 0.12094;
    pose10.orientation.w = 0.99262;
    waypoints.push_back(pose10);

    //第11个目标位姿 0/-60/120/-60/0 low-middle
    geometry_msgs::msg::Pose pose11;
    pose11.position.x = 0.11027;
    pose11.position.y = 0;
    pose11.position.z = 0.226;
    pose11.orientation.x = 0;
    pose11.orientation.y = 0;
    pose11.orientation.z = 0;
    pose11.orientation.w = 1;
    waypoints.push_back(pose11);

/* #endregion */

    move_group.setStartStateToCurrentState();
    moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
    double fraction = move_group.computeCartesianPath(waypoints, 0.01, 0.0, cartesian_plan.trajectory_);
    
    RCLCPP_INFO(logger, "Trajectory %.2f%% achieved", fraction * 100.0);
    
    if (fraction >= 0.8) {
        scale_trajectory_speed(cartesian_plan, 1.4);
        move_group.execute(cartesian_plan);
        RCLCPP_INFO(logger, "Trajectory execution completed");
        return true;
    } else {
        RCLCPP_ERROR(logger, "Failed to plan Cartesian path sufficiently");
        return false;
    }
}

bool executeCartesianPath3(moveit::planning_interface::MoveGroupInterface &move_group, rclcpp::Logger logger) {
    if (!returnToMiddle(move_group, logger)) {
        RCLCPP_ERROR(logger, "Failed to return to middle position before executing Cartesian path.");
        return false;
    }
    RCLCPP_INFO(logger, "Starting Cartesian path execution");

    /* #region  */
    std::vector<geometry_msgs::msg::Pose> waypoints;
    // 目标点0 28/-45/90/-45/0
    geometry_msgs::msg::Pose pose0;
    pose0.position.x = 0.083024;
    pose0.position.y = -0.10987;
    pose0.position.z = 0.28763;
    pose0.orientation.x = 0;
    pose0.orientation.y = 0;
    pose0.orientation.z = -0.24475;
    pose0.orientation.w = 0.96959;
    waypoints.push_back(pose0);

    // 目标点1 28/-50/90/15/0
    geometry_msgs::msg::Pose pose1;
    pose1.position.x = -0.034461;
    pose1.position.y = -0.046522;
    pose1.position.z = 0.10748;
    pose1.orientation.x = 0.11163;
    pose1.orientation.y = 0.44231;
    pose1.orientation.z = -0.21777;
    pose1.orientation.w = 0.86283;
    waypoints.push_back(pose1);

    // 目标点2 28/-60/100/10/0
    geometry_msgs::msg::Pose pose2;
    pose2.position.x = -0.033193;
    pose2.position.y = -0.047216;
    pose2.position.z = 0.096255;
    pose2.orientation.x = 0.10337;
    pose2.orientation.y = 0.40969;
    pose2.orientation.z = -0.22182;
    pose2.orientation.w = 0.87879;
    waypoints.push_back(pose2);

    // 目标点3 28/-65/110/0/0
    geometry_msgs::msg::Pose pose3;
    pose3.position.x = -0.016352;
    pose3.position.y = -0.056293;
    pose3.position.z = 0.087686;
    pose3.orientation.x = 0.093294;
    pose3.orientation.y = 0.36973;
    pose3.orientation.z = -0.22624;
    pose3.orientation.w = 0.89633;
    waypoints.push_back(pose3);

    // 目标点4 28/-70/119/-9/0
    geometry_msgs::msg::Pose pose4;
    pose4.position.x = -0.00072952;
    pose4.position.y = -0.064723;
    pose4.position.z = 0.083604;
    pose4.orientation.x = 0.082545;
    pose4.orientation.y = 0.32711;
    pose4.orientation.z = -0.23041;
    pose4.orientation.w = 0.91274;
    waypoints.push_back(pose4);

    // 目标点5 28/-78/134/-28/0 
    geometry_msgs::msg::Pose pose5;
    pose5.position.x = 0.030862;
    pose5.position.y = -0.081752;
    pose5.position.z = 0.082453;
    pose5.orientation.x = 0.059886;
    pose5.orientation.y = 0.23743;
    pose5.orientation.z = -0.23731;
    pose5.orientation.w = 0.94007;
    waypoints.push_back(pose5);

    // 目标点55 28/-78/136/-37/0 
    geometry_msgs::msg::Pose pose55;
    pose55.position.x = 0.045914;
    pose55.position.y = -0.08981;
    pose55.position.z = 0.10271;
    pose55.orientation.x = 0.045323;
    pose55.orientation.y = 0.17931;
    pose55.orientation.z = -0.24037;
    pose55.orientation.w = 0.9529;
    waypoints.push_back(pose55);

    // 目标点6 28/-78/138/-44/0
    geometry_msgs::msg::Pose pose6;
    pose6.position.x = 0.054854;
    pose6.position.y = -0.094613;
    pose6.position.z = 0.11793;
    pose6.orientation.x = 0.034817;
    pose6.orientation.y = 0.13775;
    pose6.orientation.z = -0.24206;
    pose6.orientation.w = 0.9598;
    waypoints.push_back(pose6);

    // 目标点7 28/-82/142/-52/0 半举平
    geometry_msgs::msg::Pose pose7;
    pose7.position.x = 0.061581;
    pose7.position.y = -0.098223;
    pose7.position.z = 0.14013;
    pose7.orientation.x = 0.017288;
    pose7.orientation.y = 0.068398;
    pose7.orientation.z = -0.24388;
    pose7.orientation.w = 0.96723;
    waypoints.push_back(pose7);

    // 目标点8 28/-82/145/-60/0 举平
    // 目标点8 28/-82/145/-58/0 举平
    geometry_msgs::msg::Pose pose8;
    pose8.position.x = 0.067408;
    pose8.position.y = -0.10135;
    pose8.position.z = 0.14599;
    pose8.orientation.x = 0.010242;
    pose8.orientation.y = 0.040522;
    pose8.orientation.z = -0.24425;
    pose8.orientation.w = 0.96881;
    waypoints.push_back(pose8);

    // 目标点8 28/-90/142/-50/0 刮板
    geometry_msgs::msg::Pose pose9;
    pose9.position.x = 0.053625;
    pose9.position.y = -0.093921;
    pose9.position.z = 0.16204;
    pose9.orientation.x = 0.0035733;
    pose9.orientation.y = 0.014151;
    pose9.orientation.z = -0.24446;
    pose9.orientation.w = 0.96955;
    waypoints.push_back(pose9);

    //第10个目标位姿 14/-90/142/-50/0 左移
    geometry_msgs::msg::Pose pose10;
    pose10.position.x = 0.071307;
    pose10.position.y = -0.047553;
    pose10.position.z = 0.16428;
    pose10.orientation.x = 0.0011452;
    pose10.orientation.y = 0.0093179;
    pose10.orientation.z = -0.12094;
    pose10.orientation.w = 0.99262;
    waypoints.push_back(pose10);

    //第11个目标位姿 0/-60/120/-60/0 low-middle
    geometry_msgs::msg::Pose pose11;
    pose11.position.x = 0.11027;
    pose11.position.y = 0;
    pose11.position.z = 0.226;
    pose11.orientation.x = 0;
    pose11.orientation.y = 0;
    pose11.orientation.z = 0;
    pose11.orientation.w = 1;
    waypoints.push_back(pose11);

/* #endregion */

    move_group.setStartStateToCurrentState();
    moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
    double fraction = move_group.computeCartesianPath(waypoints, 0.01, 0.0, cartesian_plan.trajectory_);
    
    RCLCPP_INFO(logger, "Trajectory %.2f%% achieved", fraction * 100.0);
    
    if (fraction >= 0.8) {
        scale_trajectory_speed(cartesian_plan, 1.4);
        move_group.execute(cartesian_plan);
        RCLCPP_INFO(logger, "Trajectory execution completed");
        return true;
    } else {
        RCLCPP_ERROR(logger, "Failed to plan Cartesian path sufficiently");
        return false;
    }
}

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    auto moveit_node = rclcpp::Node::make_shared("zc4_moveit_control");
    moveit::planning_interface::MoveGroupInterface move_group(moveit_node, "zc4");
    
    bool success = executeCartesianPath3(move_group, moveit_node->get_logger());
    
    rclcpp::shutdown();
    return 0;
}